#include <gl/glee.h>
#include <gl/GL.h>
#include <gl/GLU.h>

#include "trianglePairTestSimulator.h"

llib::Boolean TrianglePairSimulator::Initialize()
{
	reInitScene();
	return TRUE;
}

llib::Boolean TrianglePairSimulator::Render()
{
	Float currentTime	= Float(_timer.elapsed());
	Int32 curTimeStepId	= Int32(currentTime / _deltaT);
	Float timeInStep	= Float(currentTime - curTimeStepId * _deltaT);

	if( curTimeStepId != _timeStepId ) {	// this is the first frame of current time step
		framePostProcess();		// post process for last frame
		_timeStepId = curTimeStepId;
		framePreProcess();		// pre process for this frame
	}

	// set the current location and also check collison
	FOR(tid,0,TRIANGLE_NUM) {
		Triangle3f &startLoc	= _startLoc[tid];
		Triangle3f &endLoc		= _endLoc[tid];
		Triangle3f &curLoc		= _curLoc[tid];
		Triangle3f &velocity	= _velocity[tid];
		FOR(I,0,3) {
			curLoc.cell[I] = startLoc.cell[I] + velocity.cell[I] * timeInStep;
 			if( !boundInsideCheck(startLoc.cell[I], endLoc.cell[I], curLoc.cell[I]) ) {
 				// when collision occur, set current location equal to end location
 				curLoc.cell[I] = endLoc.cell[I];
 			}
		}
	}

	if( checkNewCollisionInstance(timeInStep) ) {	// first frame in this time step
		std::set<CollisionInstant>::iterator iter = _ciSetIter;
		iter--;
		const CollisionInstant &instant = *iter;
		if( instant.Type == COLLISION_TYPE_VF || instant.Type == COLLISION_TYPE_EE ) {
			// update collision corresponding triangle pair collision status
			Int32 firstTriangleId	= Int32(instant.p0 / 3);
			Int32 secondTriangleId	= Int32(instant.p2 / 3);
			Boolean collisionOccur	= TrianglePairTester::TestStatic(_curLoc[firstTriangleId], _curLoc[secondTriangleId]);
			_pairCollision[firstTriangleId]	 = collisionOccur;
			_pairCollision[secondTriangleId] = collisionOccur;
		}
	}

	// render...
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
  	gluLookAt(	0.0,  0.0, 30.0,
  				0.0,  0.0, 0.0,
  				0.0,  1.0, 0.0 );

	glBegin(GL_TRIANGLES);
	FOR(tid,0,TRIANGLE_NUM) {
		// set the right color according to collision status
		if( _pairCollision[tid] )
			glColor3fv(_invalidColor.cell);
		else
			glColor3fv(_validColor.cell);
		// set the location
		FOR(I,0,3) {
			Vector3f &currentLoc = _curLoc[tid].cell[I];
			glVertex3f(currentLoc.x, currentLoc.y, currentLoc.z);
		}
	}
	glEnd();
	glFlush();

	return TRUE;
}

llib::Boolean TrianglePairSimulator::OnKeyDown( Uint8 key, Int32 x, Int32 y, Uint8 modifiers )
{

	return TRUE;
}

void TrianglePairSimulator::reInitScene()
{
	_timer.restart();
	resetLocation();
	resetVelocity();

	_timeStepId = -1;
	_collisionInstants.clear();
	_ciSetIter	= _collisionInstants.begin();

	FOR(tid,0,TRIANGLE_NUM) {
		FOR(I,0,3) {
			FOR(J,0,3) {
				_boundCollision[tid].cell[I].cell[J] = FALSE;
			}
		}
	}

	// init pair collision
	FOR(I,0,TRIANGLE_NUM) _pairCollision[I] = FALSE;
	FOR(firstId,0,TRIANGLE_NUM) {
		for(int secondId = firstId + 1; secondId < TRIANGLE_NUM; ++secondId) {
			Boolean intersect = TrianglePairTester::TestStatic(_startLoc[firstId],_startLoc[secondId]);
			if(intersect) {
				_pairCollision[firstId]	 = TRUE;
				_pairCollision[secondId] = TRUE;
			}
		}
	}
}

Float locations[TRIANGLE_NUM][3][3] = { { {-8.0, 0.0, 9.0}, {-9.0, -1.0, 9.0}, {-9.0, 1.0, 9.0} },
										{ { 8.0, 0.0, 9.0}, { 9.0, -1.0, 8.0}, { 9.0, 1.0, 10.0} } };
Float velocitys[TRIANGLE_NUM][3][3] = { { {5.0, 0.0, 0.0}, {5.0, 0.0, 0.0}, {5.0, 0.0, 0.0} },
										{ {-5.0, 0.0, 0.0}, {-5.0, 0.0, 0.0}, {-5.0, 0.0, 0.0} } };
//Float velocitys[TRIANGLE_NUM][3][3] = { { {3.0, 0.0, 0.0}, {3.0, 0.0, 0.0}, {3.0, 0.0, 0.0} },
//										{ {-3.0, 0.0, 0.0}, {-3.0, 0.0, 0.0}, {-3.0, 0.0, 0.0} } };

void TrianglePairSimulator::resetLocation()
{
	FOR(tid,0,TRIANGLE_NUM) {
		FOR(I,0,3) {
			FOR(J,0,3) {
				_startLoc[tid].cell[I].cell[J] = _endLoc[tid].cell[I].cell[J] =
					//randomize(_locationMaxBound, _locationMinBound);
					locations[tid][I][J];
			}
		}
	}
}

void TrianglePairSimulator::resetVelocity()
{
	FOR(tid,0,TRIANGLE_NUM) {
		FOR(I,0,3) {
			FOR(J,0,3) {
				_velocity[tid].cell[I].cell[J] = velocitys[tid][I][J];//randomize(_velocityMaxBound, _velocityMinBound);
			}
		}
	}
}

llib::Float TrianglePairSimulator::randomize( Float maxBound, Float minBound )
{
	return Float( (maxBound - minBound) * math::Rnd() + minBound );
}

void TrianglePairSimulator::framePostProcess()
{
	FOR(tid,0,TRIANGLE_NUM) {
		Triangle3f &velocity		= _velocity[tid];
		Triangle3b &boundCollision	= _boundCollision[tid];
		FOR(v,0,3) {
			FOR(axis,0,3) {
				if(boundCollision.cell[v].cell[axis] == TRUE) {
					velocity.cell[v].cell[axis] *= -1;
					boundCollision.cell[v].cell[axis] = FALSE;
				}
			}
		}
	}
}

void TrianglePairSimulator::framePreProcess()
{
	_collisionInstants.clear();

	// velocity update
	// ... let's skip it first

	// get start location
	// the location in current time step is the end location of last time step
	FOR(tid,0,TRIANGLE_NUM) {
		_startLoc[tid]	= _endLoc[tid];
	}

	// get end location
	endLocationUpdate();

	// check triangle pair collision ( the KEY point )
	triangleCollisionCheck();

	// iterator need to be reset before preprocess is done
	_ciSetIter	= _collisionInstants.begin();
}

llib::Boolean TrianglePairSimulator::checkNewCollisionInstance( Float timeInStep )
{
	if( _ciSetIter == _collisionInstants.end() )
		return FALSE;
	
	const CollisionInstant& instant = *_ciSetIter;
	if( timeInStep < instant.TimePoint )
		return FALSE;

	_ciSetIter++;
	return TRUE;
}

void TrianglePairSimulator::endLocationUpdate()
{
	Float tmpLoc = 0;
	FOR(tid,0,TRIANGLE_NUM) {
		FOR(I,0,3) {	// for every point
			Boolean				boundCollisionExist = FALSE;
			Int16 axisId		= -1;	// 1:x, 2:y, 3:z
			Float collisionTime	= -1.0;

			Vector3f&			startPoint	= _startLoc[tid].cell[I];
			Vector3f&			endPoint	= _endLoc[tid].cell[I];
			Vector3f&			velocity	= _velocity[tid].cell[I];
			Vector3<Boolean>&	collision	= _boundCollision[tid].cell[I];

			FOR(J,0,3) {
				tmpLoc = Float( startPoint.cell[J] + velocity.cell[J] * _deltaT );
				endPoint.cell[J]  = tmpLoc;
				collision.cell[J] = FALSE;
				Float axisCollisionTime;
				if( tmpLoc > _locationMaxBound ) {	// above max bound
					axisCollisionTime	= (_locationMaxBound - startPoint.cell[J]) / velocity.cell[J];
				} else if( tmpLoc < _locationMinBound ) {	// below min bound
					axisCollisionTime	= (startPoint.cell[J] - _locationMinBound) / velocity.cell[J];
				} else {
					continue;	// this axis has no collision, so go on
				}
				if( !boundCollisionExist ) {
					collisionTime		= axisCollisionTime;
					axisId				= J;
					boundCollisionExist = TRUE;
				} else {
					if( axisCollisionTime < collisionTime ) {
						collisionTime	= axisCollisionTime;
						axisId			= J;
					}
				}
			}

			if( !boundCollisionExist ) continue;

			// end location need to be recalculated when collision occur on any of the axis
			FOR(J,0,3) {
				endPoint.cell[J]	= startPoint.cell[J] + velocity.cell[J] * collisionTime;
				collision.cell[J]	= (J == axisId);
			}
			CollisionInstant	instant;		// then we add a collision instant
			instant.p0			= tid * 3 + I;
			instant.TimePoint	= collisionTime;
			instant.Type		= COLLISION_TYPE_BOUND;
			_collisionInstants.insert(instant);
		}
	}
}

void TrianglePairSimulator::triangleCollisionCheck()
{
	Int32	sz = 0;
	FOR(firstId,0,TRIANGLE_NUM) {
		for(int secondId = firstId + 1; secondId < TRIANGLE_NUM; ++secondId) {
			vector<CollisionInstant> &pairCollisions
				= TrianglePairTester::TestDynamic(	_deltaT,
													_startLoc[firstId],	_velocity[firstId],
													_startLoc[secondId],_velocity[secondId]);
			// transfer the local vertex index to actural index
			Int16 firstOffset = firstId * 3;
			Int16 secondOffset = secondId * 3 - 3;
			FOR(I,0,SZ(pairCollisions)) {
				switch(pairCollisions[I].Type) {
					case COLLISION_TYPE_VF:
						pairCollisions[I].p0 += firstOffset;
						pairCollisions[I].p1 += secondOffset;
						pairCollisions[I].p2 += secondOffset;
						pairCollisions[I].p3 += secondOffset;
						break;
					case COLLISION_TYPE_EE:
						pairCollisions[I].p0 += firstOffset;
						pairCollisions[I].p1 += firstOffset;
						pairCollisions[I].p2 += secondOffset;
						pairCollisions[I].p3 += secondOffset;
						break;
					default:
						// never get here
						break;
				}
			}
			// set to collision instants set
			vector<CollisionInstant>::iterator iter = pairCollisions.begin();
			for(; iter != pairCollisions.end(); iter++) {
				_collisionInstants.insert( *iter );
			}
		}
	}

}

llib::Boolean TrianglePairSimulator::boundInsideCheck( Vector3f &start, Vector3f &end, Vector3f &current )
{
	FOR(I,0,3) {
		Float minBound, maxBound;
		if( start.cell[I] > end.cell[I] ) {
			maxBound = start.cell[I];
			minBound = end.cell[I];
		} else {
			maxBound = end.cell[I];
			minBound = start.cell[I];
		}
		if( current.cell[I] < minBound || current.cell[I] > maxBound )
			return FALSE;
	}
	return TRUE;
}
